﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Effects.Filters
{
    /// <summary>
    ///  Eight order filter with methods to create Butterworth filters.
    /// </summary>
    public class EighthOrderFilter : Filter
    {
        #region coefficients
        // normalized coefficients from transfer functions

        /// <summary>
        ///  Nominator coefficients.
        /// </summary>
        private double[] a;

        /// <summary>
        ///  Gets nominator coefficients from transfer function.
        /// </summary>
        public double[] A
        {
            get { return a; }
        }

        /// <summary>
        ///  Denominator coefficients.
        /// </summary>
        private double[] b;

        /// <summary>
        ///  Gets denominator coefficients from transfer function.
        /// </summary>
        public double[] B
        {
            get { return b; }
        }

        #endregion

        #region previous_inputs_and_outputs
        // previous Xs and Ys of the filter.

        /// <summary>
        ///  Previous inputs. 
        /// </summary>
        private double[] x;

        /// <summary>
        ///  Previous outputs.
        /// </summary>
        private double[] y;

        #endregion

        #region constructor
        // constructors 

        /// <summary>
        ///  Creates new 8t order IIR filter with given normalized coefficients.
        /// </summary>
        /// <param name="a"> Normalized nominator ceofficients. </param>
        /// <param name="b"> Normalized denominator coefficients. </param>
        /// <remarks>
        ///  All coefficients are expected directly from filter's transfer function.
        /// </remarks>
        public EighthOrderFilter(double[] a, double[] b)
        {
            if (a.Length != 9)
                throw new ArgumentException("Array containing nominator coefficients has invalid length. For 2nd order filter expectd length is 3");

            if (b.Length != 9)
                throw new ArgumentException("Array containing denominator coefficients has invalid length. For 2nd order filter expectd length is 3");

            this.a = a;
            this.b = b;

            x = new double[9];
            y = new double[9];
        }

        /// <summary>
        ///  Creates new 8th order IIR filter with given coefficients. If normalized argument is set to false it normalizes them itself.
        /// </summary>
        /// <param name="a"> Nominator coefficients. </param>
        /// <param name="b"> Denominator coefficients. </param>
        /// <param name="normalized"> Indicates whether given coefficients are normalized or not. </param>
        /// <remarks>
        ///  All coefficients are expected directly from filter's transfer function.
        /// </remarks>
        public EighthOrderFilter(double[] a, double[] b, bool normalized):this(a,b)
        {
            if (!normalized)
            {
                for (int i = 0; i < 9; i++)
                    this.a[i] /= this.b[0];

                for (int i = 1; i < 9; i++)
                    this.b[i] /= this.b[0];

                this.b[0] = 1;
            }
        }

        #endregion

        #region step_method
        // overridden step method

        /// <summary>
        ///  Filters input signal.
        /// </summary>
        /// <param name="x0"> Input signal </param>
        /// <returns> Filtered output signal </returns>
        public override double Step(double x0)
        {
            x[0] = x0;
            y[0] = 0;

            y[0] = a[0] * x[0] + a[1] * x[1] + a[2] * x[2] + a[3] * x[3] + a[4] * x[4] + a[5] * x[5] + a[6] * x[6] + a[7] * x[7] + a[8] * x[4];
            y[0] += -b[1] * y[1] - b[2] * y[2] - b[3] * y[3] - b[4] * y[4] - b[5] * y[5] - b[6] * y[6] - b[7] * y[7] - b[8] * y[8];

            // move Xs and Ys
            x[8] = x[7];
            x[7] = x[6];
            x[6] = x[5];
            x[5] = x[4];
            x[4] = x[3];
            x[3] = x[2];
            x[2] = x[1];
            x[1] = x[0];

            y[8] = y[7];
            y[7] = y[6];
            y[6] = y[5];
            y[5] = y[4];
            y[4] = y[3];
            y[3] = y[2];
            y[2] = y[1];
            y[1] = y[0];

            return y[0];
        }

        #endregion

        #region creator_methods
        // method creates different types of 2nd order filters

        /// <summary>
        ///  Constant for Butterworth filter denominator.
        /// </summary>
        private const double I = 0.3902;
        
        /// <summary>
        ///  Constant for Butterworth filter denominator.
        /// </summary>
        private const double J = 1.1111;

        /// <summary>
        ///  Constant for Butterworth filter denominator.
        /// </summary>
        private const double K = 1.6629;

        /// <summary>
        ///  Costant for Butterworth filter denominator
        /// </summary>
        private const double L = 1.9616;

        /// <summary>
        ///  Creates 8th order low-pass Butterworth filter .
        /// </summary>
        /// <param name="frequency"> Desired -3db cut-off frequency</param>
        /// <param name="sampleRate"> Sampling rate of the input signal </param>
        /// <returns> 8th order filter with appropriate coefficients </returns>
        public static EighthOrderFilter CreateLowPass(double frequency, int sampleRate)
        {
            // normalize frequency
            double normalizedFreq = frequency / sampleRate * Math.PI * 2;
            double omega = Math.Tan(normalizedFreq / 2);
            double omega2 = omega * omega;
            double omega8 = Math.Pow(omega, 8);

            double Common = 2 * (omega2 - 1);
            double Im = 1 - I * omega + omega2;
            double Ip = 1 + I * omega + omega2;
            double Jm = 1 - J * omega + omega2;
            double Jp = 1 + J * omega + omega2;
            double Km = 1 - K * omega + omega2;
            double Kp = 1 + K * omega + omega2;
            double Lm = 1 - L * omega + omega2;
            double Lp = 1 + L * omega + omega2;

            double X5 = Im * Jm;
            double X4 = Common * Jm + Common * Im;
            double X3 = Common * Common + Ip * Jm + Im * Jp;
            double X2 = Common * Ip + Common * Jp;
            double X1 = Ip * Jp;

            double Y5 = Km * Lm;
            double Y4 = Common * Lm + Common * Km;
            double Y3 = Common * Common + Kp * Lm + Km * Lp;
            double Y2 = Common * Kp + Common * Lp;
            double Y1 = Kp * Lp;

            double[] a = new double[9];
            double[] b = new double[9];

            a[0] = omega8;
            a[1] = omega8 * 8;
            a[2] = omega8 * 28;
            a[3] = omega8 * 56;
            a[4] = omega8 * 70;
            a[5] = omega8 * 56;
            a[6] = omega8 * 28;
            a[7] = omega8 * 8;
            a[8] = omega8;

            b[8] = X5 * Y5;
            b[7] = X4 * Y5 + X5 * Y4;
            b[6] = X3 * Y5 + X4 * Y4 + X5 * Y3;
            b[5] = X2 * Y5 + X3 * Y4 + X4 * Y3 + X5 * Y2;
            b[4] = X1 * Y5 + X2 * Y4 + X3 * Y3 + X4 * Y2 + X5 * Y1;
            b[3] = X1 * Y4 + X2 * Y3 + X3 * Y2 + X4 * Y1;
            b[2] = X1 * Y3 + X2 * Y2 + X3 * Y1;
            b[1] = X1 * Y2 + X2 * Y1;
            b[0] = X1 * Y1;

            // normalize coefficients
            for (int i = 0; i < 9; i++)
                a[i] = a[i] / b[0];
            for (int i = 1; i < 9; i++)
                b[i] = b[i] / b[0];
            b[0] = 1;

            return new EighthOrderFilter(a, b);
        }

        /// <summary>
        ///  Creates 8th order high-pass Butterworth filter.
        /// </summary>
        /// <param name="frequency"> Desired -3db cut-off frequency</param>
        /// <param name="sampleRate"> Sampling rate of the input signal </param>
        /// <returns> 8th order filter with appropriate coefficients </returns>
        public static EighthOrderFilter CreateHighPass(double frequency, int sampleRate)
        {
            // normalize frequency
            double normalizedFreq = (sampleRate / 2 - frequency) / sampleRate * Math.PI * 2;
            double omega = Math.Tan(normalizedFreq / 2);
            double omega2 = omega * omega;
            double omega8 = Math.Pow(omega, 8);

            double Common = 2 * (omega2 - 1);
            double Im = 1 - I * omega + omega2;
            double Ip = 1 + I * omega + omega2;
            double Jm = 1 - J * omega + omega2;
            double Jp = 1 + J * omega + omega2;
            double Km = 1 - K * omega + omega2;
            double Kp = 1 + K * omega + omega2;
            double Lm = 1 - L * omega + omega2;
            double Lp = 1 + L * omega + omega2;

            double X5 = Im * Jm;
            double X4 = Common * Jm + Common * Im;
            double X3 = Common * Common + Ip * Jm + Im * Jp;
            double X2 = Common * Ip + Common * Jp;
            double X1 = Ip * Jp;

            double Y5 = Km * Lm;
            double Y4 = Common * Lm + Common * Km;
            double Y3 = Common * Common + Kp * Lm + Km * Lp;
            double Y2 = Common * Kp + Common * Lp;
            double Y1 = Kp * Lp;

            double[] a = new double[9];
            double[] b = new double[9];

            a[0] = omega8;
            a[1] = - omega8 * 8;
            a[2] = omega8 * 28;
            a[3] = - omega8 * 56;
            a[4] = omega8 * 70;
            a[5] = - omega8 * 56;
            a[6] = omega8 * 28;
            a[7] = - omega8 * 8;
            a[8] = omega8;

            b[8] = X5 * Y5;
            b[7] = - X4 * Y5 + X5 * Y4;
            b[6] = X3 * Y5 + X4 * Y4 + X5 * Y3;
            b[5] = - X2 * Y5 + X3 * Y4 + X4 * Y3 + X5 * Y2;
            b[4] = X1 * Y5 + X2 * Y4 + X3 * Y3 + X4 * Y2 + X5 * Y1;
            b[3] = - X1 * Y4 + X2 * Y3 + X3 * Y2 + X4 * Y1;
            b[2] = X1 * Y3 + X2 * Y2 + X3 * Y1;
            b[1] = - X1 * Y2 + X2 * Y1;
            b[0] = X1 * Y1;

            // normalize coefficients
            for (int i = 0; i < 9; i++)
                a[i] = a[i] / b[0];
            for (int i = 1; i < 9; i++)
                b[i] = b[i] / b[0];
            b[0] = 1;

            return new EighthOrderFilter(a, b);
        }

        #endregion
    }
}
